

<!DOCTYPE html>
<!--[if IE 8]><html class="no-js lt-ie9" lang="en" > <![endif]-->
<!--[if gt IE 8]><!--> <html class="no-js" lang="en" > <!--<![endif]-->
<head>
  <meta charset="utf-8">
  
  <meta name="viewport" content="width=device-width, initial-scale=1.0">
  
  <title>IMU-GNSS Sensor-Fusion on the KITTI Dataset &mdash; Unscented Kalman Filtering on (Parallelizable) Manifolds alpha documentation</title>
  

  
  

  

  
  
    

  

  
  
    <link rel="stylesheet" href="../_static/css/theme.css" type="text/css" />
  

  
    <link rel="stylesheet" href="../_static/gallery.css" type="text/css" />
  
    <link rel="stylesheet" href="../_static/custom.css" type="text/css" />
  

  
        <link rel="index" title="Index"
              href="../genindex.html"/>
        <link rel="search" title="Search" href="../search.html"/>
    <link rel="top" title="Unscented Kalman Filtering on (Parallelizable) Manifolds alpha documentation" href="../index.html"/>
        <link rel="up" title="Examples" href="../examples.html"/>
        <link rel="next" title="Pendulum Example" href="pendulum.html"/>
        <link rel="prev" title="2D Robot SLAM - Example" href="slam2d.html"/> 

  
  <script src="../_static/js/modernizr.min.js"></script>

</head>

<body class="wy-body-for-nav" role="document">

  <div class="wy-grid-for-nav">

    
    <nav data-toggle="wy-nav-shift" class="wy-nav-side">
      <div class="wy-side-scroll">
        <div class="wy-side-nav-search">
          

          
            <a href="../index.html" class="icon icon-home"> Unscented Kalman Filtering on (Parallelizable) Manifolds
          

          
            
            <img src="../_static/blacklogo.png" class="logo" />
          
          </a>

          
            
            
              <div class="version">
                1.0
              </div>
            
          

          
<div role="search">
  <form id="rtd-search-form" class="wy-form" action="../search.html" method="get">
    <input type="text" name="q" placeholder="Search docs" />
    <input type="hidden" name="check_keywords" value="yes" />
    <input type="hidden" name="area" value="default" />
  </form>
</div>

          
        </div>

        <div class="wy-menu wy-menu-vertical" data-spy="affix" role="navigation" aria-label="main navigation">
          
            
            
                <ul class="current">
<li class="toctree-l1"><a class="reference internal" href="../install.html">Installation</a></li>
<li class="toctree-l1"><a class="reference internal" href="localization.html">Tutorial</a></li>
<li class="toctree-l1 current"><a class="reference internal" href="../examples.html">Examples</a><ul class="current">
<li class="toctree-l2"><a class="reference internal" href="localization.html">2D Robot Localization - Tutorial</a></li>
<li class="toctree-l2"><a class="reference internal" href="wifibot.html">2D Robot Localization on Real Data</a></li>
<li class="toctree-l2"><a class="reference internal" href="attitude.html">Attitude Estimation with an IMU</a></li>
<li class="toctree-l2"><a class="reference internal" href="inertial_navigation.html">Navigation on Flat Earth</a></li>
<li class="toctree-l2"><a class="reference internal" href="slam2d.html">2D Robot SLAM</a></li>
<li class="toctree-l2 current"><a class="current reference internal" href="#">IMU-GNSS Sensor-Fusion on the KITTI Dataset</a><ul>
<li class="toctree-l3"><a class="reference internal" href="#import">Import</a></li>
<li class="toctree-l3"><a class="reference internal" href="#model-and-data">Model and Data</a><ul>
<li class="toctree-l4"><a class="reference internal" href="#filter-design-and-initialization">Filter Design and Initialization</a></li>
</ul>
</li>
<li class="toctree-l3"><a class="reference internal" href="#filtering">Filtering</a><ul>
<li class="toctree-l4"><a class="reference internal" href="#results">Results</a></li>
</ul>
</li>
<li class="toctree-l3"><a class="reference internal" href="#conclusion">Conclusion</a></li>
</ul>
</li>
<li class="toctree-l2"><a class="reference internal" href="pendulum.html">Pendulum Example</a></li>
</ul>
</li>
<li class="toctree-l1"><a class="reference internal" href="../benchmarks.html">Benchmarks</a></li>
<li class="toctree-l1"><a class="reference internal" href="../filter.html">Filters</a></li>
<li class="toctree-l1"><a class="reference internal" href="../model.html">Models</a></li>
<li class="toctree-l1"><a class="reference internal" href="../geometry.html">Lie Groups</a></li>
<li class="toctree-l1"><a class="reference internal" href="../matlab.html">Matlab</a></li>
<li class="toctree-l1"><a class="reference internal" href="../license.html">License</a></li>
<li class="toctree-l1"><a class="reference internal" href="../bibliography.html">Bibliography</a></li>
</ul>

            
          
        </div>
      </div>
    </nav>

    <section data-toggle="wy-nav-shift" class="wy-nav-content-wrap">

      
      <nav class="wy-nav-top" role="navigation" aria-label="top navigation">
        <i data-toggle="wy-nav-top" class="fa fa-bars"></i>
        <a href="../index.html">Unscented Kalman Filtering on (Parallelizable) Manifolds</a>
      </nav>


      
      <div class="wy-nav-content">
        <div class="rst-content">
          

 



<div role="navigation" aria-label="breadcrumbs navigation">
  <ul class="wy-breadcrumbs">
    <li><a href="../index.html">Docs</a> &raquo;</li>
      
          <li><a href="../examples.html">Examples</a> &raquo;</li>
      
    <li>IMU-GNSS Sensor-Fusion on the KITTI Dataset</li>
      <li class="wy-breadcrumbs-aside">
        
          
            <a href="../_sources/auto_examples/imugnss.rst.txt" rel="nofollow"> View page source</a>
          
        
      </li>
  </ul>
  <hr/>
</div>
          <div role="main" class="document" itemscope="itemscope" itemtype="http://schema.org/Article">
           <div itemprop="articleBody">
            
  <div class="sphx-glr-download-link-note admonition note">
<p class="admonition-title">Note</p>
<p>Click <a class="reference internal" href="#sphx-glr-download-auto-examples-imugnss-py"><span class="std std-ref">here</span></a> to download the full example code</p>
</div>
<div class="sphx-glr-example-title section" id="imu-gnss-sensor-fusion-on-the-kitti-dataset">
<span id="sphx-glr-auto-examples-imugnss-py"></span><h1>IMU-GNSS Sensor-Fusion on the KITTI Dataset<a class="headerlink" href="#imu-gnss-sensor-fusion-on-the-kitti-dataset" title="Permalink to this headline">¶</a></h1>
<p>Goals of this script:</p>
<ul class="simple">
<li><p>apply the UKF for estimating the 3D pose, velocity and sensor biases of a
vehicle on real data.</p></li>
<li><p>efficiently propagate the filter when one part of the Jacobian is already
known.</p></li>
<li><p>efficiently update the system for GNSS position.</p></li>
</ul>
<p><em>We assume the reader is already familiar with the approach described in the
tutorial and in the 2D SLAM example.</em></p>
<p>This script proposes an UKF to estimate the 3D attitude, the velocity, and the
position of a rigid body in space from inertial sensors and position
measurement.</p>
<p>We use the KITTI data that can be found in the <a class="reference external" href="https://github.com/borglab/gtsam/blob/develop/matlab/gtsam_examples/IMUKittiExampleGNSS.m">iSAM repo</a>
(examples folder).</p>
<div class="section" id="import">
<h2>Import<a class="headerlink" href="#import" title="Permalink to this headline">¶</a></h2>
<div class="highlight-default notranslate"><div class="highlight"><pre><span></span><span class="kn">from</span> <span class="nn">scipy.linalg</span> <span class="k">import</span> <span class="n">block_diag</span>
<span class="kn">import</span> <span class="nn">ukfm</span>
<span class="kn">import</span> <span class="nn">numpy</span> <span class="k">as</span> <span class="nn">np</span>
<span class="kn">import</span> <span class="nn">matplotlib</span>
<span class="n">ukfm</span><span class="o">.</span><span class="n">set_matplotlib_config</span><span class="p">()</span>
</pre></div>
</div>
</div>
<div class="section" id="model-and-data">
<h2>Model and Data<a class="headerlink" href="#model-and-data" title="Permalink to this headline">¶</a></h2>
<p>This script uses the <a class="reference internal" href="../model.html#ukfm.IMUGNSS" title="ukfm.IMUGNSS"><code class="xref py py-meth docutils literal notranslate"><span class="pre">IMUGNSS()</span></code></a> model that loads the KITTI data
from text files. The model is the standard 3D kinematics model based on
inertial inputs.</p>
<div class="highlight-default notranslate"><div class="highlight"><pre><span></span><span class="n">MODEL</span> <span class="o">=</span> <span class="n">ukfm</span><span class="o">.</span><span class="n">IMUGNSS</span>
<span class="c1"># observation frequency (Hz)</span>
<span class="n">GNSS_freq</span> <span class="o">=</span> <span class="mi">1</span>
<span class="c1"># load data</span>
<span class="n">omegas</span><span class="p">,</span> <span class="n">ys</span><span class="p">,</span> <span class="n">one_hot_ys</span><span class="p">,</span> <span class="n">t</span> <span class="o">=</span> <span class="n">MODEL</span><span class="o">.</span><span class="n">load</span><span class="p">(</span><span class="n">GNSS_freq</span><span class="p">)</span>
<span class="n">N</span> <span class="o">=</span> <span class="n">t</span><span class="o">.</span><span class="n">shape</span><span class="p">[</span><span class="mi">0</span><span class="p">]</span>
<span class="c1"># IMU noise standard deviation (noise is isotropic)</span>
<span class="n">imu_std</span> <span class="o">=</span> <span class="n">np</span><span class="o">.</span><span class="n">array</span><span class="p">([</span><span class="mf">0.01</span><span class="p">,</span>     <span class="c1"># gyro (rad/s)</span>
                    <span class="mf">0.05</span><span class="p">,</span>     <span class="c1"># accelerometer (m/s^2)</span>
                    <span class="mf">0.000001</span><span class="p">,</span> <span class="c1"># gyro bias (rad/s^2)</span>
                    <span class="mf">0.0001</span><span class="p">])</span>  <span class="c1"># accelerometer bias (m/s^3)</span>
<span class="c1"># GNSS noise standard deviation (m)</span>
<span class="n">GNSS_std</span> <span class="o">=</span> <span class="mf">0.05</span>
</pre></div>
</div>
<p>The state and the input contain the following variables:</p>
<div class="highlight-python notranslate"><div class="highlight"><pre><span></span><span class="n">states</span><span class="p">[</span><span class="n">n</span><span class="p">]</span><span class="o">.</span><span class="n">Rot</span>     <span class="c1"># 3d orientation (matrix)</span>
<span class="n">states</span><span class="p">[</span><span class="n">n</span><span class="p">]</span><span class="o">.</span><span class="n">v</span>       <span class="c1"># 3d velocity</span>
<span class="n">states</span><span class="p">[</span><span class="n">n</span><span class="p">]</span><span class="o">.</span><span class="n">p</span>       <span class="c1"># 3d position</span>
<span class="n">states</span><span class="p">[</span><span class="n">n</span><span class="p">]</span><span class="o">.</span><span class="n">b_gyro</span>  <span class="c1"># gyro bias</span>
<span class="n">states</span><span class="p">[</span><span class="n">n</span><span class="p">]</span><span class="o">.</span><span class="n">b_acc</span>   <span class="c1"># accelerometer bias</span>
<span class="n">omegas</span><span class="p">[</span><span class="n">n</span><span class="p">]</span><span class="o">.</span><span class="n">gyro</span>    <span class="c1"># vehicle angular velocities</span>
<span class="n">omegas</span><span class="p">[</span><span class="n">n</span><span class="p">]</span><span class="o">.</span><span class="n">acc</span>     <span class="c1"># vehicle specific forces</span>
</pre></div>
</div>
<p>A measurement <code class="docutils literal notranslate"><span class="pre">ys[k]</span></code> contains a GNSS (position) measurement.</p>
<div class="section" id="filter-design-and-initialization">
<h3>Filter Design and Initialization<a class="headerlink" href="#filter-design-and-initialization" title="Permalink to this headline">¶</a></h3>
<p>We now design the UKF on parallelizable manifolds. This script embeds the
state in <span class="math notranslate nohighlight">\(SO(3) \times \mathbb{R}^{12}\)</span>, such that:</p>
<ul class="simple">
<li><p>the retraction <span class="math notranslate nohighlight">\(\varphi(.,.)\)</span> is the <span class="math notranslate nohighlight">\(SO(3)\)</span> exponential for
orientation, and the vector addition for the remaining part of the
state.</p></li>
<li><p>the inverse retraction <span class="math notranslate nohighlight">\(\varphi^{-1}_.(.)\)</span> is the <span class="math notranslate nohighlight">\(SO(3)\)</span>
logarithm for orientation and the vector subtraction for the remaining part
of the state.</p></li>
</ul>
<p>Remaining parameter setting is standard.</p>
<div class="highlight-default notranslate"><div class="highlight"><pre><span></span><span class="c1"># propagation noise covariance matrix</span>
<span class="n">Q</span> <span class="o">=</span> <span class="n">block_diag</span><span class="p">(</span><span class="n">imu_std</span><span class="p">[</span><span class="mi">0</span><span class="p">]</span><span class="o">**</span><span class="mi">2</span><span class="o">*</span><span class="n">np</span><span class="o">.</span><span class="n">eye</span><span class="p">(</span><span class="mi">3</span><span class="p">),</span> <span class="n">imu_std</span><span class="p">[</span><span class="mi">1</span><span class="p">]</span><span class="o">**</span><span class="mi">2</span><span class="o">*</span><span class="n">np</span><span class="o">.</span><span class="n">eye</span><span class="p">(</span><span class="mi">3</span><span class="p">),</span>
               <span class="n">imu_std</span><span class="p">[</span><span class="mi">2</span><span class="p">]</span><span class="o">**</span><span class="mi">2</span><span class="o">*</span><span class="n">np</span><span class="o">.</span><span class="n">eye</span><span class="p">(</span><span class="mi">3</span><span class="p">),</span> <span class="n">imu_std</span><span class="p">[</span><span class="mi">3</span><span class="p">]</span><span class="o">**</span><span class="mi">2</span><span class="o">*</span><span class="n">np</span><span class="o">.</span><span class="n">eye</span><span class="p">(</span><span class="mi">3</span><span class="p">))</span>
<span class="c1"># measurement noise covariance matrix</span>
<span class="n">R</span> <span class="o">=</span> <span class="n">GNSS_std</span><span class="o">**</span><span class="mi">2</span> <span class="o">*</span> <span class="n">np</span><span class="o">.</span><span class="n">eye</span><span class="p">(</span><span class="mi">3</span><span class="p">)</span>
</pre></div>
</div>
<p>We use the UKF that is able to infer Jacobian to speed up the update step, see
the 2D SLAM example.</p>
<div class="highlight-default notranslate"><div class="highlight"><pre><span></span><span class="c1"># sigma point parameters</span>
<span class="n">alpha</span> <span class="o">=</span> <span class="n">np</span><span class="o">.</span><span class="n">array</span><span class="p">([</span><span class="mf">1e-3</span><span class="p">,</span> <span class="mf">1e-3</span><span class="p">,</span> <span class="mf">1e-3</span><span class="p">,</span> <span class="mf">1e-3</span><span class="p">,</span> <span class="mf">1e-3</span><span class="p">])</span>
<span class="c1"># for propagation we need the all state</span>
<span class="n">red_idxs</span> <span class="o">=</span> <span class="n">np</span><span class="o">.</span><span class="n">arange</span><span class="p">(</span><span class="mi">15</span><span class="p">)</span>  <span class="c1"># indices corresponding to the full state in P</span>
<span class="c1"># for update we need only the state corresponding to the position</span>
<span class="n">up_idxs</span> <span class="o">=</span> <span class="n">np</span><span class="o">.</span><span class="n">array</span><span class="p">([</span><span class="mi">6</span><span class="p">,</span> <span class="mi">7</span><span class="p">,</span> <span class="mi">8</span><span class="p">])</span>
</pre></div>
</div>
<p>We initialize the state with zeros biases. The initial covariance is non-null
as the state is not perfectly known.</p>
<div class="highlight-default notranslate"><div class="highlight"><pre><span></span><span class="c1"># initial uncertainty matrix</span>
<span class="n">P0</span> <span class="o">=</span> <span class="n">block_diag</span><span class="p">(</span><span class="mf">0.01</span><span class="o">*</span><span class="n">np</span><span class="o">.</span><span class="n">eye</span><span class="p">(</span><span class="mi">3</span><span class="p">),</span> <span class="mi">1</span><span class="o">*</span><span class="n">np</span><span class="o">.</span><span class="n">eye</span><span class="p">(</span><span class="mi">3</span><span class="p">),</span> <span class="mi">1</span><span class="o">*</span><span class="n">np</span><span class="o">.</span><span class="n">eye</span><span class="p">(</span><span class="mi">3</span><span class="p">),</span>
                <span class="mf">0.001</span><span class="o">*</span><span class="n">np</span><span class="o">.</span><span class="n">eye</span><span class="p">(</span><span class="mi">3</span><span class="p">),</span> <span class="mf">0.001</span><span class="o">*</span><span class="n">np</span><span class="o">.</span><span class="n">eye</span><span class="p">(</span><span class="mi">3</span><span class="p">))</span>
<span class="c1"># initial state</span>
<span class="n">state0</span> <span class="o">=</span> <span class="n">MODEL</span><span class="o">.</span><span class="n">STATE</span><span class="p">(</span>
    <span class="n">Rot</span><span class="o">=</span><span class="n">np</span><span class="o">.</span><span class="n">eye</span><span class="p">(</span><span class="mi">3</span><span class="p">),</span>
    <span class="n">v</span><span class="o">=</span><span class="n">np</span><span class="o">.</span><span class="n">zeros</span><span class="p">(</span><span class="mi">3</span><span class="p">),</span>
    <span class="n">p</span><span class="o">=</span><span class="n">np</span><span class="o">.</span><span class="n">zeros</span><span class="p">(</span><span class="mi">3</span><span class="p">),</span>
    <span class="n">b_gyro</span><span class="o">=</span><span class="n">np</span><span class="o">.</span><span class="n">zeros</span><span class="p">(</span><span class="mi">3</span><span class="p">),</span>
    <span class="n">b_acc</span><span class="o">=</span><span class="n">np</span><span class="o">.</span><span class="n">zeros</span><span class="p">(</span><span class="mi">3</span><span class="p">))</span>
</pre></div>
</div>
<p>As the noise affecting the dynamic of the biases is trivial (it is the
identity), we set our UKF with a reduced propagation noise covariance, and
manually set the remaining part of the Jacobian.</p>
<div class="highlight-default notranslate"><div class="highlight"><pre><span></span><span class="c1"># create the UKF</span>
<span class="n">ukf</span> <span class="o">=</span> <span class="n">ukfm</span><span class="o">.</span><span class="n">JUKF</span><span class="p">(</span><span class="n">state0</span><span class="o">=</span><span class="n">state0</span><span class="p">,</span> <span class="n">P0</span><span class="o">=</span><span class="n">P0</span><span class="p">,</span> <span class="n">f</span><span class="o">=</span><span class="n">MODEL</span><span class="o">.</span><span class="n">f</span><span class="p">,</span> <span class="n">h</span><span class="o">=</span><span class="n">MODEL</span><span class="o">.</span><span class="n">h</span><span class="p">,</span> <span class="n">Q</span><span class="o">=</span><span class="n">Q</span><span class="p">[:</span><span class="mi">6</span><span class="p">,</span> <span class="p">:</span><span class="mi">6</span><span class="p">],</span>
                <span class="n">phi</span><span class="o">=</span><span class="n">MODEL</span><span class="o">.</span><span class="n">phi</span><span class="p">,</span> <span class="n">alpha</span><span class="o">=</span><span class="n">alpha</span><span class="p">,</span> <span class="n">red_phi</span><span class="o">=</span><span class="n">MODEL</span><span class="o">.</span><span class="n">phi</span><span class="p">,</span>
                <span class="n">red_phi_inv</span><span class="o">=</span><span class="n">MODEL</span><span class="o">.</span><span class="n">phi_inv</span><span class="p">,</span> <span class="n">red_idxs</span><span class="o">=</span><span class="n">red_idxs</span><span class="p">,</span>
                <span class="n">up_phi</span><span class="o">=</span><span class="n">MODEL</span><span class="o">.</span><span class="n">up_phi</span><span class="p">,</span> <span class="n">up_idxs</span><span class="o">=</span><span class="n">up_idxs</span><span class="p">)</span>
<span class="c1"># set variables for recording estimates along the full trajectory</span>
<span class="n">ukf_states</span> <span class="o">=</span> <span class="p">[</span><span class="n">state0</span><span class="p">]</span>
<span class="n">ukf_Ps</span> <span class="o">=</span> <span class="n">np</span><span class="o">.</span><span class="n">zeros</span><span class="p">((</span><span class="n">N</span><span class="p">,</span> <span class="mi">15</span><span class="p">,</span> <span class="mi">15</span><span class="p">))</span>
<span class="n">ukf_Ps</span><span class="p">[</span><span class="mi">0</span><span class="p">]</span> <span class="o">=</span> <span class="n">P0</span>
<span class="c1"># the part of the Jacobian that is already known.</span>
<span class="n">G_const</span> <span class="o">=</span> <span class="n">np</span><span class="o">.</span><span class="n">zeros</span><span class="p">((</span><span class="mi">15</span><span class="p">,</span> <span class="mi">6</span><span class="p">))</span>
<span class="n">G_const</span><span class="p">[</span><span class="mi">9</span><span class="p">:]</span> <span class="o">=</span> <span class="n">np</span><span class="o">.</span><span class="n">eye</span><span class="p">(</span><span class="mi">6</span><span class="p">)</span>
</pre></div>
</div>
</div>
</div>
<div class="section" id="filtering">
<h2>Filtering<a class="headerlink" href="#filtering" title="Permalink to this headline">¶</a></h2>
<p>The UKF proceeds as a standard Kalman filter with a for loop.</p>
<div class="highlight-default notranslate"><div class="highlight"><pre><span></span><span class="c1"># measurement iteration number</span>
<span class="n">k</span> <span class="o">=</span> <span class="mi">1</span>
<span class="k">for</span> <span class="n">n</span> <span class="ow">in</span> <span class="nb">range</span><span class="p">(</span><span class="mi">1</span><span class="p">,</span> <span class="n">N</span><span class="p">):</span>
    <span class="c1"># propagation</span>
    <span class="n">dt</span> <span class="o">=</span> <span class="n">t</span><span class="p">[</span><span class="n">n</span><span class="p">]</span><span class="o">-</span><span class="n">t</span><span class="p">[</span><span class="n">n</span><span class="o">-</span><span class="mi">1</span><span class="p">]</span>
    <span class="n">ukf</span><span class="o">.</span><span class="n">state_propagation</span><span class="p">(</span><span class="n">omegas</span><span class="p">[</span><span class="n">n</span><span class="o">-</span><span class="mi">1</span><span class="p">],</span> <span class="n">dt</span><span class="p">)</span>
    <span class="n">ukf</span><span class="o">.</span><span class="n">F_num</span><span class="p">(</span><span class="n">omegas</span><span class="p">[</span><span class="n">n</span><span class="o">-</span><span class="mi">1</span><span class="p">],</span> <span class="n">dt</span><span class="p">)</span>
    <span class="c1"># we assert the reduced noise covariance for computing Jacobian.</span>
    <span class="n">ukf</span><span class="o">.</span><span class="n">Q</span> <span class="o">=</span> <span class="n">Q</span><span class="p">[:</span><span class="mi">6</span><span class="p">,</span> <span class="p">:</span><span class="mi">6</span><span class="p">]</span>
    <span class="n">ukf</span><span class="o">.</span><span class="n">G_num</span><span class="p">(</span><span class="n">omegas</span><span class="p">[</span><span class="n">n</span><span class="o">-</span><span class="mi">1</span><span class="p">],</span> <span class="n">dt</span><span class="p">)</span>
    <span class="c1"># concatenate Jacobian</span>
    <span class="n">ukf</span><span class="o">.</span><span class="n">G</span> <span class="o">=</span> <span class="n">np</span><span class="o">.</span><span class="n">hstack</span><span class="p">((</span><span class="n">ukf</span><span class="o">.</span><span class="n">G</span><span class="p">,</span> <span class="n">G_const</span><span class="o">*</span><span class="n">dt</span><span class="p">))</span>
    <span class="c1"># we assert the full noise covariance for uncertainty propagation.</span>
    <span class="n">ukf</span><span class="o">.</span><span class="n">Q</span> <span class="o">=</span> <span class="n">Q</span>
    <span class="n">ukf</span><span class="o">.</span><span class="n">cov_propagation</span><span class="p">()</span>
    <span class="c1"># update only if a measurement is received</span>
    <span class="k">if</span> <span class="n">one_hot_ys</span><span class="p">[</span><span class="n">n</span><span class="p">]</span> <span class="o">==</span> <span class="mi">1</span><span class="p">:</span>
        <span class="n">ukf</span><span class="o">.</span><span class="n">update</span><span class="p">(</span><span class="n">ys</span><span class="p">[</span><span class="n">k</span><span class="p">],</span> <span class="n">R</span><span class="p">)</span>
        <span class="n">k</span> <span class="o">=</span> <span class="n">k</span> <span class="o">+</span> <span class="mi">1</span>
    <span class="c1"># save estimates</span>
    <span class="n">ukf_states</span><span class="o">.</span><span class="n">append</span><span class="p">(</span><span class="n">ukf</span><span class="o">.</span><span class="n">state</span><span class="p">)</span>
    <span class="n">ukf_Ps</span><span class="p">[</span><span class="n">n</span><span class="p">]</span> <span class="o">=</span> <span class="n">ukf</span><span class="o">.</span><span class="n">P</span>
</pre></div>
</div>
<div class="section" id="results">
<h3>Results<a class="headerlink" href="#results" title="Permalink to this headline">¶</a></h3>
<p>We plot the estimated trajectory.</p>
<div class="highlight-default notranslate"><div class="highlight"><pre><span></span><span class="n">MODEL</span><span class="o">.</span><span class="n">plot_results</span><span class="p">(</span><span class="n">ukf_states</span><span class="p">,</span> <span class="n">ys</span><span class="p">)</span>
</pre></div>
</div>
<img alt="../_images/sphx_glr_imugnss_001.png" class="sphx-glr-single-img" src="../_images/sphx_glr_imugnss_001.png" />
<p>Results are coherent with the GNSS. As the GNSS is used in the filter, it
makes no sense to compare the filter outputs to the same measurement.</p>
</div>
</div>
<div class="section" id="conclusion">
<h2>Conclusion<a class="headerlink" href="#conclusion" title="Permalink to this headline">¶</a></h2>
<p>This script implements an UKF for sensor-fusion of an IMU with GNSS. The UKF
is efficiently implemented, as some part of the Jacobian are known and not
computed. Results are satisfying.</p>
<p>You can now:</p>
<ul class="simple">
<li><p>increase the difficulties of the example by reduced the GNSS frequency or
adding noise to position measurements.</p></li>
<li><p>implement the UKF with different uncertainty representations, as viewing the
state as an element <span class="math notranslate nohighlight">\(\boldsymbol{\chi} \in SE_2(3) \times
\mathbb{R}^6\)</span>. We yet provide corresponding retractions and inverse
retractions.</p></li>
</ul>
<p class="sphx-glr-timing"><strong>Total running time of the script:</strong> ( 5 minutes  19.978 seconds)</p>
<div class="sphx-glr-footer class sphx-glr-footer-example docutils container" id="sphx-glr-download-auto-examples-imugnss-py">
<div class="sphx-glr-download docutils container">
<p><a class="reference download internal" download="" href="../_downloads/66bfa94f89f38a97f472167e79076486/imugnss.py"><code class="xref download docutils literal notranslate"><span class="pre">Download</span> <span class="pre">Python</span> <span class="pre">source</span> <span class="pre">code:</span> <span class="pre">imugnss.py</span></code></a></p>
</div>
<div class="sphx-glr-download docutils container">
<p><a class="reference download internal" download="" href="../_downloads/d3740909c02838cd0c146ae6634ed841/imugnss.ipynb"><code class="xref download docutils literal notranslate"><span class="pre">Download</span> <span class="pre">Jupyter</span> <span class="pre">notebook:</span> <span class="pre">imugnss.ipynb</span></code></a></p>
</div>
</div>
<p class="sphx-glr-signature"><a class="reference external" href="https://sphinx-gallery.github.io">Gallery generated by Sphinx-Gallery</a></p>
</div>
</div>


           </div>
          </div>
          <footer>
  
    <div class="rst-footer-buttons" role="navigation" aria-label="footer navigation">
      
        <a href="pendulum.html" class="btn btn-neutral float-right" title="Pendulum Example" accesskey="n">Next <span class="fa fa-arrow-circle-right"></span></a>
      
      
        <a href="slam2d.html" class="btn btn-neutral" title="2D Robot SLAM - Example" accesskey="p"><span class="fa fa-arrow-circle-left"></span> Previous</a>
      
    </div>
  

  <hr/>

  <div role="contentinfo">
    <p>
        &copy; Copyright 2019, Martin Brossard, Axel Barrau, Silvère Bonnabel.

    </p>
  </div>
  Built with <a href="http://sphinx-doc.org/">Sphinx</a> using a <a href="https://github.com/snide/sphinx_rtd_theme">theme</a> provided by <a href="https://readthedocs.org">Read the Docs</a>. 

</footer>

        </div>
      </div>

    </section>

  </div>
  


  

    <script type="text/javascript">
        var DOCUMENTATION_OPTIONS = {
            URL_ROOT:'../',
            VERSION:'alpha',
            COLLAPSE_INDEX:false,
            FILE_SUFFIX:'.html',
            HAS_SOURCE:  true
        };
    </script>
      <script type="text/javascript" src="../_static/jquery.js"></script>
      <script type="text/javascript" src="../_static/underscore.js"></script>
      <script type="text/javascript" src="../_static/doctools.js"></script>
      <script type="text/javascript" src="../_static/language_data.js"></script>
      <script type="text/javascript" src="https://cdnjs.cloudflare.com/ajax/libs/mathjax/2.7.5/latest.js?config=TeX-AMS-MML_HTMLorMML"></script>

  

  
  
    <script type="text/javascript" src="../_static/js/theme.js"></script>
  

  
  
  <script type="text/javascript">
      jQuery(function () {
          SphinxRtdTheme.StickyNav.enable();
      });
  </script>
   

</body>
</html>